import numpy as np

# 读取文件
truth_file = "gnss_ground_truth.txt"
est_file = "laser_odom.txt"
output_truth = "gnss_ground_truth_filtered.txt"
output_est = "laser_odom_filtered.txt"
max_diff = 0.1  # 最大允许时间差

# 加载数据
truth = np.loadtxt(truth_file)
est = np.loadtxt(est_file)

truth_ts = truth[:, 0]
est_ts = est[:, 0]

matched_truth = []
matched_est = []

for t in truth:
    ts_truth = t[0]  # 真值时间戳
    diffs = np.abs(est_ts - ts_truth)
    min_idx = np.argmin(diffs)
    if diffs[min_idx] <= max_diff:
        # 关键修改：用真值的时间戳覆盖估计轨迹的时间戳（确保时间戳完全一致）
        est_matched = est[min_idx].copy()
        est_matched[0] = ts_truth  # 估计轨迹的时间戳 = 真值时间戳
        matched_truth.append(t)
        matched_est.append(est_matched)

# 保存格式（无末尾空格）
fmt = "%.9f %.6f %.6f %.6f %.6f %.6f %.6f %.6f"
np.savetxt(output_truth, np.array(matched_truth), fmt=fmt)
np.savetxt(output_est, np.array(matched_est), fmt=fmt)

print(f"筛选完成：共保留 {len(matched_truth)} 组匹配的时间戳（时间戳已强制对齐）")